﻿/*
 * This file is part of MonoStrategy.
 *
 * Copyright (C) 2010-2011 Christoph Husse
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Affero General Public License as
 *  published by the Free Software Foundation, either version 3 of the
 *  License, or (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Affero General Public License for more details.
 *
 *  You should have received a copy of the GNU Affero General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Authors: 
 *      # Christoph Husse
 * 
 * Also checkout our homepage: http://monostrategy.codeplex.com/
 */
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MonoStrategy
{
    internal partial class TerrainRouting
    {
        private class CooperativeAStar
        {
            private SortedDictionary<PathNodeKey, Object> m_ClosedSet = new SortedDictionary<PathNodeKey, Object>(PathNodeKey.Comparer);
            private SortedDictionary<PathNodeKey, Object> m_OpenSet = new SortedDictionary<PathNodeKey, Object>(PathNodeKey.Comparer);
            private SortedDictionary<MovablePathNode, MovablePathNode> m_Reservation = new SortedDictionary<MovablePathNode, MovablePathNode>(MovablePathNode.Comparer);
            private PriorityQueue<PathNodeKey> m_OrderedOpenSet;
            private CameFromMap m_CameFrom = new CameFromMap();
            private AStarSolver m_ObstacleScanner;
            private int m_Size;
            private int m_SizeShift;
            private Byte[] m_UserGrid;
            private readonly PathNodeKey[] m_Neighbors = new PathNodeKey[7];
            PathNodeKey m_StartNode;
            private const Double SQRT_2 = 1.41421356;

            public TerrainRouting SearchSpace { get; private set; }

            public CooperativeAStar(TerrainRouting inParent)
            {
                double log2Factor = 1.0 / Math.Log(2.0);

                SearchSpace = inParent;
                m_Size = inParent.Size;
                m_Size = inParent.Size;
                m_SizeShift = (int)Math.Floor((Math.Log((double)m_Size) * log2Factor) + 0.5);
                m_OrderedOpenSet = new PriorityQueue<PathNodeKey>();
                //m_ObstacleScanner = new AStarSolver(SearchSpace.Terrain);
            }

            private double GetNeighborDistance(PathNodeKey inStart, PathNodeKey inEnd)
            {
                int diffX = Math.Abs(inStart.X - inEnd.X);
                int diffY = Math.Abs(inStart.Y - inEnd.Y);

                switch (diffX + diffY)
                {
                    case 1: return 1;
                    case 2: return SQRT_2;
                    default:
                        return 1;
                }
            }

            public void UnregisterPathNode(MovablePathNode inPathNode)
            {
                m_Reservation.Remove(inPathNode);
            }

            public void UnregisterPath(LinkedList<MovablePathNode> inPath)
            {
                foreach (var entry in inPath)
                {
                    UnregisterPathNode(entry);
                }
            }

            /// <summary>
            /// Returns null, if no path is found. Start- and End-Node are included in returned path. The user context
            /// is passed to IsWalkable().
            /// </summary>
            public void Search(Movable inMovable, PathNodeKey inCurrent, PathNodeKey inTarget, WallValue inWallBreaker)
            {
                SearchInternal(inMovable, inCurrent, inTarget, inWallBreaker, inWithCooperation: true, inWithWallBreaking: false);
            }

            private static Int64 s_ProcessedNodes = 0;
            private static Int64 s_ProcessCounter = 0;
            private static Int64 s_CoopFallbackCounter = 0;
            private static Int64 s_WallBreakFallbackCounter = 0;

            private void SearchInternal(Movable inMovable, PathNodeKey inCurrent, PathNodeKey inTarget, WallValue inWallBreaker, bool inWithCooperation, bool inWithWallBreaking)
            {
                s_ProcessCounter++;

                MovablePathNode keyNode = new MovablePathNode();
                long cycleResolution = SearchSpace.CycleResolution;

                m_StartNode = inCurrent;
                m_UserGrid = SearchSpace.Terrain.GetWallMap();

                m_ClosedSet.Clear();
                m_OpenSet.Clear();
                m_OrderedOpenSet.Clear();
                m_CameFrom.Clear();

                m_StartNode.Time = SearchSpace.CurrentCycle;
                m_StartNode.G = 0;
                m_StartNode.H = SearchSpace.GetHeuristic(new Point(inTarget.X, inTarget.Y), new Point(inCurrent.X, inCurrent.Y));
                m_StartNode.F = m_StartNode.H;
                m_StartNode.Length = 0;

                m_OpenSet.Add(m_StartNode, null);
                m_OrderedOpenSet.Push(m_StartNode);
                
                while (m_OpenSet.Count > 0)
                {
                    PathNodeKey x = m_OrderedOpenSet.Pop();

                    if (x.Length >= 6)
                    {
                        ReconstructPath(inCurrent, m_CameFrom, m_CameFrom[x], inMovable.Path);

                        if (inWithCooperation)
                        {


                            foreach (var entry in inMovable.Path)
                            {
                                entry.Movable = inMovable;

                                //m_Reservation.Remove(entry);
                                //m_Reservation.Add(entry, entry);

                                /* TODO: Also add an entry for the opposite direction, if not already there.
                                 * This will prevent two colliding movables from walking through each
                                 * other on a straight line, which is a common case, since well...
                                 * it is optimal ;) but still not desired.
                                 */
                            }
                        }

                        return;
                    }

                    m_OpenSet.Remove(x);
                    m_ClosedSet.Add(x, null);

                    s_ProcessedNodes++;

                    StoreNeightbors(x, cycleResolution);

                   // bool isOnReservedCell = m_UserGrid[x.X + (x.Y << m_SizeShift)] >= (int)WallValue.Reserved;

                    for (int i = 0; i < m_Neighbors.Length - ((inWithWallBreaking) ? 1 : 0); i++)
                    {
                        PathNodeKey y = m_Neighbors[i];
                        Boolean tentative_is_better;

                        if (m_ClosedSet.ContainsKey(y))
                            continue;

                        if (inWithCooperation)
                        {
                            MovablePathNode reservation;

                            keyNode.Time = y.Time;
                            keyNode.Position = new Point(y.X, y.Y);

                            if (m_Reservation.TryGetValue(keyNode, out reservation))
                            {
                                if (reservation.Movable.HasJob)
                                {
                                    continue;
                                }
                                else
                                {
                                    if (!inMovable.HasJob)
                                    {
                                        continue;
                                    }

                                    reservation.Movable.IsInvalidated = true;
                                }
                            }
                        }

                        if (!inWithWallBreaking)
                        {
                            if (m_UserGrid[y.X + (y.Y << m_SizeShift)] >= (int)WallValue.WaterBorder)
                                continue;
                        }

                        Double tentative_g_score = x.G + GetNeighborDistance(x, y);
                        Boolean wasAdded = false;

                        if (!m_OpenSet.ContainsKey(y))
                        {
                            m_OpenSet.Add(y, null);
                            tentative_is_better = true;
                            wasAdded = true;
                        }
                        else if (tentative_g_score < y.G)
                        {
                            tentative_is_better = true;
                        }
                        else
                        {
                            tentative_is_better = false;
                        }

                        if (tentative_is_better)
                        {
                            m_CameFrom.Add(y, x);

                            y.G = tentative_g_score;
                            y.H = SearchSpace.GetHeuristic(new Point(inTarget.X, inTarget.Y), new Point(y.X, y.Y));
                            y.F = y.G + y.H;

                            if (wasAdded)
                                m_OrderedOpenSet.Push(y);
                            else
                                m_OrderedOpenSet.Update(y);
                        }
                    }
                }

                if (inWithCooperation)
                {
                    s_CoopFallbackCounter++;
                    SearchInternal(inMovable, inCurrent, inTarget, inWallBreaker, inWithCooperation: false, inWithWallBreaking: false);
                }
                else if (!inWithWallBreaking)
                {
                    s_WallBreakFallbackCounter++;
                    SearchInternal(inMovable, inCurrent, inTarget, inWallBreaker, inWithCooperation: false, inWithWallBreaking: true);
                }
                else
                    throw new ArgumentException("No path could be found!");
            }

            private void StoreNeightbors(PathNodeKey inAround, long inCycleResolution)
            {
                long time = inAround.Time + inCycleResolution;
                int newLen = inAround.Length + 1;

                m_Neighbors[6] = new PathNodeKey(inAround.X, inAround.Y, time, newLen);
                m_Neighbors[1] = new PathNodeKey(inAround.X + 1, inAround.Y, time, newLen);
                m_Neighbors[2] = new PathNodeKey(inAround.X + 1, inAround.Y + 1, time, newLen);
                m_Neighbors[3] = new PathNodeKey(inAround.X - 1, inAround.Y, time, newLen);
                m_Neighbors[4] = new PathNodeKey(inAround.X - 1, inAround.Y + 1, time, newLen);
                m_Neighbors[5] = new PathNodeKey(inAround.X + 1, inAround.Y - 1, time, newLen);
                m_Neighbors[0] = new PathNodeKey(inAround.X - 1, inAround.Y - 1, time, newLen);
            }

            private void ReconstructPath(PathNodeKey inPosition, CameFromMap came_from, PathNodeKey current_node, LinkedList<MovablePathNode> outPath)
            {
                long time = current_node.Time;
                PathNodeKey lastNodeKey;

                came_from.GetPath(current_node, outPath);

                lastNodeKey = new PathNodeKey(current_node.X, current_node.Y, time);

                Direction? lastDirection = null;
                LinkedListNode<MovablePathNode> node = outPath.First;

                while (node != null)
                {
                    LinkedListNode<MovablePathNode> nextNode = node.Next;

                    if (nextNode != null)
                    {
                        Direction? dir = DirectionUtils.GetWalkingDirection(node.Value.Position, nextNode.Value.Position);

                        lastDirection = node.Value.Direction = dir;
                    }
                    else
                        node.Value.Direction = lastDirection;

                    node = nextNode;
                }
            }
        }
    }
}